

Foot L1;
Foot L2;
Foot L3;
Foot R1;
Foot R2;
Foot R3;
int main_step;
int t;
void Main(string arguments){
    if(R1 == null){GetBlocks();  return;}

    t ++;
    
    if(arguments == "a"){
        main_step ++;
        if(main_step == 4) main_step = 0;
    }

    R1.AttitudeInit();
    R2.AttitudeInit();
    R3.AttitudeInit();
    L1.AttitudeInit();
    L2.AttitudeInit();
    L3.AttitudeInit();

    if(main_step == 1){
        R1.JY1.MoveTo(7);
        R1.JY2.MoveTo(5);
        R3.JY1.MoveTo(7);
        R3.JY2.MoveTo(5);
        L2.JY1.MoveTo(7);
        L2.JY2.MoveTo(5);
    }else if(main_step == 2){
        R1.JY1.MoveTo(7);
        R1.JY2.MoveTo(5);
        R3.JY1.MoveTo(7);
        R3.JY2.MoveTo(5);
        L2.JY1.MoveTo(7);
        L2.JY2.MoveTo(5);
        R2.JY1.MoveTo(7);
        R2.JY2.MoveTo(5);
        L1.JY1.MoveTo(7);
        L1.JY2.MoveTo(5);
        L3.JY1.MoveTo(7);
        L3.JY2.MoveTo(5);
    }else if(main_step == 3){
        R1.JY1.MoveTo(2);
        R1.JY2.MoveTo(3);
        R3.JY1.MoveTo(2);
        R3.JY2.MoveTo(3);
        L2.JY1.MoveTo(2);
        L2.JY2.MoveTo(3);
        R2.JY1.MoveTo(2);
        R2.JY2.MoveTo(3);
        L1.JY1.MoveTo(2);
        L1.JY2.MoveTo(3);
        L3.JY1.MoveTo(2);
        L3.JY2.MoveTo(3);
    }
    Echo(R1.JumpStep.ToString());
}

void GetBlocks(){
    // 获取X关节
    Joint f1_jt1 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_1", "Joint_X_Rotor_1")), 
        new List<Rotor>(
            new Rotor[]{new Rotor(getGroupBlocksByBlockName("Foot_1", "Joint_X_Rotor_2")), 
            new Rotor(getGroupBlocksByBlockName("Foot_1", "Joint_X_Rotor_3"))}
        )
    );
    f1_jt1.MoveTo_PID_P = 20;
    Joint f1_jt2 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_1", "Joint_Y1_Rotor")), 
        new List<Piston>(
            new Piston[]{new Piston(getGroupBlocksByBlockName("Foot_1", "Joint_Y1_Piston_1")), 
            new Piston(getGroupBlocksByBlockName("Foot_1", "Joint_Y1_Piston_2"))}
        )
    );
    Joint f1_jt3 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_1", "Joint_Y2_Rotor")), 
        new List<Piston>(
            new Piston[]{new Piston(getGroupBlocksByBlockName("Foot_1", "Joint_Y2_Piston_1")), 
            new Piston(getGroupBlocksByBlockName("Foot_1", "Joint_Y2_Piston_2"))}
        )
    );
    R1 = new Foot(f1_jt1, f1_jt2, f1_jt3);
    R1.LeftRight = -1;

    // 获取X关节
    Joint f2_jt1 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_2", "Joint_X_Rotor_1")), 
        new List<Rotor>(
            new Rotor[]{new Rotor(getGroupBlocksByBlockName("Foot_2", "Joint_X_Rotor_2")), 
            new Rotor(getGroupBlocksByBlockName("Foot_2", "Joint_X_Rotor_3"))}
        )
    );
    f2_jt1.MoveTo_PID_P = 20;
    Joint f2_jt2 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_2", "Joint_Y1_Rotor")), 
        new List<Piston>(
            new Piston[]{new Piston(getGroupBlocksByBlockName("Foot_2", "Joint_Y1_Piston_1")), 
            new Piston(getGroupBlocksByBlockName("Foot_2", "Joint_Y1_Piston_2"))}
        )
    );
    Joint f2_jt3 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_2", "Joint_Y2_Rotor")), 
        new List<Piston>(
            new Piston[]{new Piston(getGroupBlocksByBlockName("Foot_2", "Joint_Y2_Piston_1")), 
            new Piston(getGroupBlocksByBlockName("Foot_2", "Joint_Y2_Piston_2"))}
        )
    );
    R2 = new Foot(f2_jt1, f2_jt2, f2_jt3);
    R2.LeftRight = -1;

    // 获取X关节
    Joint f3_jt1 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_3", "Joint_X_Rotor_1")), 
        new List<Rotor>(
            new Rotor[]{new Rotor(getGroupBlocksByBlockName("Foot_3", "Joint_X_Rotor_2")), 
            new Rotor(getGroupBlocksByBlockName("Foot_3", "Joint_X_Rotor_3"))}
        )
    );
    f3_jt1.MoveTo_PID_P = 20;
    Joint f3_jt2 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_3", "Joint_Y1_Rotor")), 
        new List<Piston>(
            new Piston[]{new Piston(getGroupBlocksByBlockName("Foot_3", "Joint_Y1_Piston_1")), 
            new Piston(getGroupBlocksByBlockName("Foot_3", "Joint_Y1_Piston_2"))}
        )
    );
    Joint f3_jt3 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_3", "Joint_Y2_Rotor")), 
        new List<Piston>(
            new Piston[]{new Piston(getGroupBlocksByBlockName("Foot_3", "Joint_Y2_Piston_1")), 
            new Piston(getGroupBlocksByBlockName("Foot_3", "Joint_Y2_Piston_2"))}
        )
    );
    R3 = new Foot(f3_jt1, f3_jt2, f3_jt3);
    R3.LeftRight = -1;

    // 获取X关节
    Joint f4_jt1 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_4", "Joint_X_Rotor_1")), 
        new List<Rotor>(
            new Rotor[]{new Rotor(getGroupBlocksByBlockName("Foot_4", "Joint_X_Rotor_2")), 
            new Rotor(getGroupBlocksByBlockName("Foot_4", "Joint_X_Rotor_3"))}
        )
    );
    f4_jt1.MoveTo_PID_P = 20;
    Joint f4_jt2 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_4", "Joint_Y1_Rotor")), 
        new List<Piston>(
            new Piston[]{new Piston(getGroupBlocksByBlockName("Foot_4", "Joint_Y1_Piston_1")), 
            new Piston(getGroupBlocksByBlockName("Foot_4", "Joint_Y1_Piston_2"))}
        )
    );
    Joint f4_jt3 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_4", "Joint_Y2_Rotor")), 
        new List<Piston>(
            new Piston[]{new Piston(getGroupBlocksByBlockName("Foot_4", "Joint_Y2_Piston_1")), 
            new Piston(getGroupBlocksByBlockName("Foot_4", "Joint_Y2_Piston_2"))}
        )
    );
    L1 = new Foot(f4_jt1, f4_jt2, f4_jt3);

    // 获取X关节
    Joint f5_jt1 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_5", "Joint_X_Rotor_1")), 
        new List<Rotor>(
            new Rotor[]{new Rotor(getGroupBlocksByBlockName("Foot_5", "Joint_X_Rotor_2")), 
            new Rotor(getGroupBlocksByBlockName("Foot_5", "Joint_X_Rotor_3"))}
        )
    );
    f5_jt1.MoveTo_PID_P = 20;
    Joint f5_jt2 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_5", "Joint_Y1_Rotor")), 
        new List<Piston>(
            new Piston[]{new Piston(getGroupBlocksByBlockName("Foot_5", "Joint_Y1_Piston_1")), 
            new Piston(getGroupBlocksByBlockName("Foot_5", "Joint_Y1_Piston_2"))}
        )
    );
    Joint f5_jt3 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_5", "Joint_Y2_Rotor")), 
        new List<Piston>(
            new Piston[]{new Piston(getGroupBlocksByBlockName("Foot_5", "Joint_Y2_Piston_1")), 
            new Piston(getGroupBlocksByBlockName("Foot_5", "Joint_Y2_Piston_2"))}
        )
    );
    L2 = new Foot(f5_jt1, f5_jt2, f5_jt3);

    // 获取X关节
    Joint f6_jt1 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_6", "Joint_X_Rotor_1")), 
        new List<Rotor>(
            new Rotor[]{new Rotor(getGroupBlocksByBlockName("Foot_6", "Joint_X_Rotor_2")), 
            new Rotor(getGroupBlocksByBlockName("Foot_6", "Joint_X_Rotor_3"))}
        )
    );
    f6_jt1.MoveTo_PID_P = 20;
    Joint f6_jt2 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_6", "Joint_Y1_Rotor")), 
        new List<Piston>(
            new Piston[]{new Piston(getGroupBlocksByBlockName("Foot_6", "Joint_Y1_Piston_1")), 
            new Piston(getGroupBlocksByBlockName("Foot_6", "Joint_Y1_Piston_2"))}
        )
    );
    Joint f6_jt3 = new Joint(new Rotor(getGroupBlocksByBlockName("Foot_6", "Joint_Y2_Rotor")), 
        new List<Piston>(
            new Piston[]{new Piston(getGroupBlocksByBlockName("Foot_6", "Joint_Y2_Piston_1")), 
            new Piston(getGroupBlocksByBlockName("Foot_6", "Joint_Y2_Piston_2"))}
        )
    );
    L3 = new Foot(f6_jt1, f6_jt2, f6_jt3);
}

//从分组中获取方块的辅助方法
IMyTerminalBlock getGroupBlocksByBlockName(string group_name, string block_name){
	List<IMyBlockGroup> groups = new List<IMyBlockGroup>();
	GridTerminalSystem.GetBlockGroups(groups);
	foreach(IMyBlockGroup g in groups){
		if(g.Name == group_name){
			List<IMyTerminalBlock> blocks = new List<IMyTerminalBlock>();
			g.GetBlocks(blocks);
			foreach(IMyTerminalBlock b in blocks){
				if(b.CustomName == block_name){
					return b;
				}
			}
		}
	}
	return null;
}

//足类
class Foot
{
    public Joint JX;
    public Joint JY1;
    public Joint JY2;
    public int LeftRight = 1; //左右腿x运动反转系数

    public Foot(Joint jx, Joint jy1, Joint jy2){
        this.JX = jx;
        this.JY1 = jy1;
        this.JY2 = jy2;
    }

    public bool AttitudeInit(){
        bool a = this.JX.MoveTo(180);
        bool b = this.JY1.MoveTo(3);
        bool c = this.JY2.MoveTo(3);
        return a && b && c;
    }

    private int MoveStep = 0;
    public void Move(){
        if(this.MoveStep == 0){
            bool a = this.JX.MoveTo(170);
            if(a){
                this.MoveStep = 1;
            }
        }
        else if(this.MoveStep == 1){
            bool a = this.JX.MoveTo(200);
            bool b = this.JY1.MoveTo(7.5);
            bool c = this.JY2.MoveTo(2);
            if(a && b && c){
                this.MoveStep = 2;
            }
        }
        else if(this.MoveStep == 2){
            bool a = this.JY1.MoveTo(1);
            bool b = this.JY2.MoveTo(5);
            if(a && b){
                this.MoveStep = 0;
            }
        }
    }

    public int JumpStep = 0;
    public void Jump(){
        this.JX.MoveTo(180);
        if(this.JumpStep == 0){
            bool a = this.JY1.MoveTo(7);
            bool b = this.JY2.MoveTo(2);
            if(a && b){
                this.JumpStep = 1;
            }
        }else if(this.JumpStep == 1){
            bool a = this.JY1.MoveTo(2);
            bool b = this.JY2.MoveTo(7);
            if(a && b){
                this.JumpStep = 2;
            }
        }
    }
}

//关节类
class Joint
{
    public Rotor RotorMain;//主转子
    public List<Rotor> Rotors; //从转子
    public int RotorMainPosNeg = 1; //主转子标定正反系数，1或-1
    public List<Piston> Pistons; //活塞组
    public int Type; //类型，1是纯转子，2是纯活塞，3是转子+活塞
    
    // == 构造方法
    public Joint(Rotor main_rotor, List<Rotor> rotors){
        this.Type = 1;
        this.RotorMain = main_rotor;
        this.Rotors = rotors;
    }
    public Joint(Rotor main_rotor, List<Piston> pistons){
        this.Type = 2;
        this.RotorMain = main_rotor;
        this.Pistons = pistons;
    }
    
    // == 成员属性
    public double Angle{ //当前角度
        get { return this.RotorMain.Angle; }
    }
    public double Length{ //当前长度（仅type=2时有意义）
        get {
            if(this.Pistons.Count > 0){
                return this.Pistons[0].CurrentPosition;
            }
            return 0;
        }
    }
    public double Velocity{ //转速
        get {
            if(this.Type == 1){
                return this.RotorMain.Velocity;
            }else if(this.Type == 2){
                return this.Pistons[0].Velocity;
            }
            return 0;
        }
        set {
            if(this.Type == 1){
                this.RotorMain.Velocity = value;
                foreach(Rotor r in this.Rotors){
                    r.Velocity = value;
                }
            }else if(this.Type == 2){
                foreach(Piston p in this.Pistons){
                    p.Velocity = value;
                }
            }
        }
    }

    // == 成员方法
    //
    public double MoveTo_PID_T = 20;
    public double MoveTo_PID_P = 1;
    public double MoveTo_PID_I = 1;
    public double MoveTo_PID_D = 1;
    public double MoveTo_Accuracy = 0.5; //归位判定精度，type=1时是角度，type=2时是长度
    public double MoveTo_AccuracyVelocity = 0.2; //归位判定速度
    private List<double> MoveTo_PID_DATA = new List<double>();
    public bool MoveTo(double x = 0, double velocity = 0, int move_type = 0){
        double err = 0;
        double err_sum = 0;
        double output = 0;

        if(this.Type == 1){

            //归一化输入值
            x = Rotor.NormalizeAngle(x);

            //计算转动路径路径
            // move_type 是计算后的转动方式，它为1时是顺时针，为-1时是逆时针

            if(move_type == 0){
                if(Math.Abs(x - this.Angle) <= 180){
                    //如果当前与目标点直接弧长小于半周长
                    move_type = x - this.Angle > 0 ? 1 : -1;
                }else{
                    move_type = x - this.Angle > 0 ? -1 : 1;
                }
            }

            // err是当前角度和目标角度的转动角度差，范围是0-1
            err = 0;
            if(move_type == 1){
                if(x > this.Angle){
                    err = x - this.Angle;
                }else{
                    err = (360 - this.Angle) + x;
                }
            }else if(move_type == -1){
                if(this.Angle > x){
                    err = this.Angle - x;
                }else{
                    err = this.Angle + (360 - x);
                }
            }
            err /= 360;

            MoveTo_PID_DATA.Add(err);
            if(MoveTo_PID_DATA.Count > MoveTo_PID_T){
                MoveTo_PID_DATA.Remove(MoveTo_PID_DATA[0]);
            }else{
                MoveTo_PID_DATA.Add(err);
            }

            err_sum = 0;
            foreach(double d in MoveTo_PID_DATA){
                err_sum += d;
            }
            err_sum /= MoveTo_PID_T;

            //这里来个PID就成了
            output = MoveTo_PID_P * (err + err_sum*MoveTo_PID_I + (MoveTo_PID_DATA[MoveTo_PID_DATA.Count - 1] - MoveTo_PID_DATA[MoveTo_PID_DATA.Count - 1])*MoveTo_PID_D);
            
            //计算结果输出量
            this.Velocity = move_type*this.RotorMainPosNeg*output;

            return err*360 <= this.MoveTo_Accuracy && Math.Abs(this.Velocity) <= this.MoveTo_AccuracyVelocity;
        }
        else if(this.Type == 2){

            err = x - this.Length; //err范围是10

            MoveTo_PID_DATA.Add(err);
            if(MoveTo_PID_DATA.Count > MoveTo_PID_T){
                MoveTo_PID_DATA.Remove(MoveTo_PID_DATA[0]);
            }else{
                MoveTo_PID_DATA.Add(err);
            }

            err_sum = 0;
            foreach(double d in MoveTo_PID_DATA){
                err_sum += d;
            }
            err_sum /= MoveTo_PID_T;

            output = MoveTo_PID_P * (err + err_sum*MoveTo_PID_I + (MoveTo_PID_DATA[MoveTo_PID_DATA.Count - 1] - MoveTo_PID_DATA[MoveTo_PID_DATA.Count - 1])*MoveTo_PID_D);
            
            this.Velocity = this.RotorMainPosNeg*output;

            return Math.Abs(err) <= this.MoveTo_Accuracy && Math.Abs(this.Velocity) <= this.MoveTo_AccuracyVelocity;
        }
        return false;
    }
}

class Block
{
    // == 构造方法
    public Block(IMyTerminalBlock b){
        this._block = b;
    }

    // == 成员属性
    public IMyTerminalBlock _block; //方块
    public long Id{ //唯一ID
        get { return this._block.EntityId; }
    }
    public bool IsOK{ //是否正确获取到方块
        get { return this._block != null; }
    }
    public bool IsBeingHacked{ //是否被黑了
        get { return this._block.IsBeingHacked; }
    }
    public bool IsFunctional{ //是否可以工作
        get { return this._block.IsFunctional; }
    }
    public bool IsWorking{ //是否在工作
        get { return this._block.IsWorking; }
    }
    public int NumberInGrid{ //在网格中的顺序
        get { return this._block.NumberInGrid; }
    }
    public IMyCubeGrid Grid{ //所属网格
        get { return this._block.CubeGrid; }
    }
    public string Name{ //自定义名称（CustomName）
        get { return this._block.CustomName; }
        set { this._block.CustomName = value; }
    }
    public string Data{ //自定义数据 （CustomData）
        get { return this._block.CustomData; }
        set { this._block.CustomData = value; }
    }
    public long OwnerId{ //所有人ID
        get { return this._block.OwnerId; }
    }
    public Vector3D Position{ //坐标（基于世界绝对坐标系的坐标）
        get { return this._block.GetPosition(); }
    }
    // public WorldMatrix Matrix{ //矩阵（描述方块世界坐标几方位的矩阵）
    //     get { return this._block.WorldMatrix; }
    // }
    public Vector3D Forward{ //前向矢量（归一化的，即矢量的长度恒等于1）
        get { return this._block.WorldMatrix.Forward; }
    }
    public Vector3D Backward{ //后向矢量（归一化的，即矢量的长度恒等于1）
        get { return this._block.WorldMatrix.Backward; }
    }
    public Vector3D Left{ //左向矢量（归一化的，即矢量的长度恒等于1）
        get { return this._block.WorldMatrix.Left; }
    }
    public Vector3D Right{ //右向矢量（归一化的，即矢量的长度恒等于1）
        get { return this._block.WorldMatrix.Right; }
    }
    public Vector3D Up{ //上向矢量（归一化的，即矢量的长度恒等于1）
        get { return this._block.WorldMatrix.Up; }
    }
    public Vector3D Down{ //下向矢量（归一化的，即矢量的长度恒等于1）
        get { return this._block.WorldMatrix.Down; }
    }
    public bool ShowInTerminal{ //是否在终端显示
        get { return this._block.ShowInTerminal; }
        set { this._block.ShowInTerminal = value; }
    }
    public bool ShowInInventory{ //是否在库存中显示
        get { return this._block.ShowInInventory; }
        set { this._block.ShowInInventory = value; }
    }
}

class Rotor: Block
{
    // v1.0.0
    // 依赖：Block类

    // == 构造方法
    public Rotor(IMyTerminalBlock b):base(b){
        this._block = b;
        this._rotor = b as IMyMotorStator;
    }
    public Rotor(IMyMotorStator b):base(b){
        this._block = b;
        this._rotor = b as IMyMotorStator;
    }
    // == 静态方法
    // 角度转换
    public static double AngleToRad(double angle){
        return angle*Math.PI/180;
    }
    public static double RadToAngle(double rad){
        return rad*180/Math.PI;
    }
    public static double NormalizeAngle(double angle){
        if(angle > 360){
            return angle%360;
        }else if(angle < 0){
            return 360 + (angle%360);
        }else{
            return angle;
        }
    }


    // == 成员属性
    public IMyMotorStator _rotor; //转子方块
    public double Angle{
        get { return Rotor.RadToAngle(this._rotor.Angle); }
    }
    public double AngleRad{
        get { return this._rotor.Angle; }
    }
    public double Velocity{
        get { return this._rotor.TargetVelocityRPM; }
        set { this._rotor.TargetVelocityRPM = (float)value; }
    }
    public double VelocityRad{
        get { return this._rotor.TargetVelocityRad; }
        set { this._rotor.TargetVelocityRad = (float)value; }
    }
    public double VelocityAngle{
        get { return Rotor.RadToAngle(this._rotor.TargetVelocityRad); }
        set { this._rotor.TargetVelocityRad = (float)Rotor.AngleToRad(value); }
    }

    // == 成员方法

    // 转到某个角度
    // input angle 目标角度，使用角度而不是弧度
    // input mode 运动方式，1顺时针，-1逆时针，0自动
    // input velocity 指定速度，不指定默认用PID
    public double TurnToAngleAccuracy = 0.2; //转动抵达判定角度
    public double TurnToVelocityAccuracy = 0.01; //转动抵达判定速度
    public double turn_pid_p = 20; //比例系数
    public double turn_pid_i = 20; //积分系数(增加后降低动态误差，增加震荡)
    public double turn_pid_d = 30; //微分系数（增加后降低震荡）
    public double turn_pid_t = 30; //误差累加控制周期
    private List<double> turn_pid_diff_data = new List<double>();
    public bool TurnTo(double angle = 0, int mode = 0, double velocity = 0){
        angle = Rotor.NormalizeAngle(angle);

        if(mode == 0){
            if(angle > this.Angle){
                mode = angle - this.Angle > 180 ? -1 : 1;
            }else{
                mode = this.Angle - angle > 180 ? 1 : -1;
            }
        }

        //指定速度方式
        if(velocity != 0){
            this.Velocity = mode * velocity;
            return Math.Abs(angle - this.Angle) <= 0.5;
        }

        // diff是当前角度和目标角度的转动角度差，范围是0-1
        double diff = 0;
        if(mode == 1){
            if(angle > this.Angle){
                diff = angle - this.Angle;
            }else{
                diff = (360 - this.Angle) + angle;
            }
        }else if(mode == -1){
            if(this.Angle > angle){
                diff = this.Angle - angle;
            }else{
                diff = this.Angle + (360 - angle);
            }
        }
        diff /= 360;

        //对角度差进行逐帧累加，累加次数为 PID控制积分系数
        double diff_i = 0; //积分求和项
        if(this.turn_pid_diff_data.Count >= this.turn_pid_t){
            this.turn_pid_diff_data.Remove(this.turn_pid_diff_data[0]);
        }
        this.turn_pid_diff_data.Add(diff);
        foreach(double d in this.turn_pid_diff_data){
            diff_i += d;
        }

        //输出结果是
        this.Velocity = mode*this.turn_pid_p*(diff + diff_i*this.turn_pid_i/turn_pid_t + (diff - this.turn_pid_diff_data[this.turn_pid_diff_data.Count-1])*this.turn_pid_d);

        return diff*360 <= this.TurnToAngleAccuracy && Math.Abs(this.Velocity) <= this.TurnToVelocityAccuracy;
    }
    
}

class Piston: Block
{
    // v1.0.0
    // 依赖：Block类

    // == 构造方法
    public Piston(IMyTerminalBlock b):base(b){
        this._block = b;
        this._piston = b as IMyPistonBase;
    }
    public Piston(IMyPistonBase b):base(b){
        this._block = b;
        this._piston = b as IMyPistonBase;
    }

    // == 成员属性
    public IMyPistonBase _piston; //活塞方块本体
    public double Velocity{ //速度
        get { return this._piston.Velocity; }
        set { this._piston.Velocity = (float)value; }
    }
    public double MaxVelocity{ //最大速度
        get { return this._piston.MaxVelocity; }
    }
    public double MinLimit{ //最小限制
        get { return this._piston.MinLimit; }
        set { this._piston.MinLimit = (float)value; }
    }
    public double MaxLimit{ //最大限制
        get { return this._piston.MaxLimit; }
        set { this._piston.MaxLimit = (float)value; }
    }
    public double CurrentPosition{ //当前位置
        get { return this._piston.CurrentPosition; }
    }
    public PistonStatus Status{ //当前状态
        get { return this._piston.Status; }
    }
}

Program(){
	Runtime.UpdateFrequency = UpdateFrequency.Update1;
}